#include "sdcard.h"
#include "os_io.h"
#include "interrupt_controller.h"

static volatile uint8_t g_cmd_timeout = 0;
static sd_command_response_t null_rsp;

static inline void build_sd_command_response(sd_expected_response_t expected_response, sd_command_response_t *rsp)
{
	uint32_t response_regs[8];
	uint32_t cidx;

	

	*rsp = null_rsp;		// zero the response

	response_regs[0] = MMC_RSP01;
	response_regs[1] = (response_regs[0] & 0xFF00) >>16;
	response_regs[0] = response_regs[0] & 0xFF;

	response_regs[2] = MMC_RSP23;
	response_regs[3] = (response_regs[2] & 0xFF00) >>16;
	response_regs[2] = response_regs[2] & 0xFF;

	response_regs[4] = MMC_RSP45;
	response_regs[5] = (response_regs[4] & 0xFF00) >>16;
	response_regs[4] = response_regs[4] & 0xFF;

	response_regs[6] = MMC_RSP67;
	response_regs[7] = (response_regs[6] & 0xFF00) >>16;
	response_regs[6] = response_regs[6] & 0xFF;

	cidx = MMC_CIDX;

	printf("response regs0 = 0x%x\n", response_regs[0]);
	printf("response regs1 = 0x%x\n", response_regs[1]);
	printf("response regs2 = 0x%x\n", response_regs[2]);
	printf("response regs3 = 0x%x\n", response_regs[3]);
	printf("response regs4 = 0x%x\n", response_regs[4]);
	printf("response regs5 = 0x%x\n", response_regs[5]);
	printf("response regs6 = 0x%x\n", response_regs[6]);
	printf("response regs7 = 0x%x\n", response_regs[7]);
	printf("CIDX = 0x%x\n", cidx);


	switch(expected_response)
	{
		case R1:
		case R3:
		case R6:
			rsp->end_bit = response_regs[5] & 0x01;
			rsp->r1_r3_r6.crc7 = (response_regs[6] & 0xFE) >>1;
			rsp->r1_r3_r6.value = (response_regs[7] << 16) | (response_regs[6]);
			rsp->r1_r3_r6.command_index = cidx & 0x3F;
			rsp->transmission_bit = (cidx & 0x40) >> 6;
			rsp->start_bit = (cidx & 0x80) >>7;

			break;

		case R2:
			rsp->end_bit = response_regs[0] & 0x01;
			rsp->r2.cid_csr0 = (response_regs[0] >> 1 ) | \
					   (response_regs[1] << 15) | \
					   ((response_regs[2] & 0x01) << 31);
			rsp->r2.cid_csr1 = (response_regs[2] >> 1 ) | \
					   (response_regs[3] << 15) | \
					   ((response_regs[4] & 0x01) << 31);
			rsp->r2.cid_csr2 = (response_regs[4] >> 1 ) | \
					   (response_regs[5] << 15) | \
					   ((response_regs[6] & 0x01) << 31);
			rsp->r2.cid_csr3 = (response_regs[6] >> 1 ) | \
					   (response_regs[7] << 15);
			rsp->transmission_bit = (cidx & 0x40) >> 6;
			rsp->start_bit = (cidx & 0x80) >> 7;
			break;

		default:
			printf("ERROR!!. Expected response not defined: 0x%x\n", expected_response);
			break;
	}
			
	return;
}

static inline void handle_sdcard_timeout(void)
{
	g_cmd_timeout = 1;
	return;
}

	

static inline void run_sd_command(sd_command_t const *cmd, sd_command_response_t *rsp)
{

	uint8_t expected_rsp = 0;

	rsp->timedout = rsp->busy = 0;

	MMC_CIDX = 0;
	MMC_ARGHL = cmd->arg;

	switch(cmd->expected_rsp)
	{
		case R1:
		case R4:
		case R5:
		case R6:
			expected_rsp = 1;
			break;

		case NO_RESPONSE:
			expected_rsp = 0;
			break;

		case R2:
			expected_rsp = 2;
			break;

		case R3:
			expected_rsp = 3;
			break;

		default:
			printf("Invalid expected response: 0x%x\n", cmd->expected_rsp);
			break;
	}

	uint32_t temp = ((cmd->command & 0x3F) | ((cmd->push_pull & 0x1) << 7) | ((cmd->busy_expected & 0x01) << 8) | ((expected_rsp & 0x03) << 9) | ((cmd->read_write & 0x1) << 11) | ((cmd->stream_enable & 0x1) << 12) | ((cmd->data_transfer & 0x1) << 13) | \
		  ((cmd->init_clock & 0x1) << 14) | ((cmd->trx_clear & 0x1) << 15) | ((cmd->transfer_triggered & 0x01) << 16));
	printf("Sending command: 0x%x\n", temp);
	MMC_CMD = temp;

	// Wait for response
	if (cmd->expected_rsp != NO_RESPONSE)
	{
		uint32_t a = 0;
		g_cmd_timeout = 0;
		while ((a = MMC_ST0) == 0 && g_cmd_timeout == 0);
		if (g_cmd_timeout)
		{
			rsp->timedout = 1;
			return;
		}

		if (a & 0x04)
		{
			// RSP done
			printf("Some response received\n");
			build_sd_command_response(cmd->expected_rsp, rsp);
			return;
		}

		if (a & 0x02)
		{
			// Busy signal received
			rsp->busy = 1;
			return;
		}
	}

	return;
}

void sdcard_interrupt_handler(void)
{	
	ARM_INTC_SICR = 16;		// Clear interrupt
	
	printf("Some interrupt occured: 0x%x\n", MMC_ST0);
	return;
}

static void detect_card(void)
{
	sd_command_t cmd;
	sd_command_response_t rsp;
	
	RESET_COMMAND(cmd);

	// CMD0: Put all connected cards in reset
	cmd.command = 0;	// index 0
	cmd.init_clock = 1;	// Insert 80 clock cycles to allow card to get ready if in intermediate state
	cmd.expected_rsp = NO_RESPONSE;
	run_sd_command(&cmd, &rsp);

	// ACMD41 => Send CMD55 first
	RESET_COMMAND(cmd);
	cmd.command = 55;	// or is it 0x55 ??
	// RCA (arg) remains 0 for the command
	//#warning: Commenting busy expected while sending app command
	//cmd.busy_expected = 1;	// busy can be there if still not ready
	cmd.expected_rsp = R1;
	cmd.init_clock = 1;		// Insert 80 clock cycles to allow card to get ready after reset
	run_sd_command(&cmd, &rsp);

	if (rsp.timedout || rsp.busy) {
		printf("Timedout or busy\n");
	} else {
		printf("Found response for command: 0x%x with status: 0x%x\n", rsp.r1_r3_r6.command_index, \
				rsp.r1_r3_r6.value);
	}


	return;
}

void sdcard_init(void)
{
	uint8_t i = 0;

	// Build default null response
	for (i = 0; i < sizeof(null_rsp); i++)
	{
		*(((uint8_t *)&null_rsp) + i) = 0;
	}


	MMC_CTL = 0x03;	// put controller in reset
	printf("MMC_CTL is: 0x%x\n", MMC_CTL);
	MMC_CLK = 0x00;

	MMC_CTL |= 0x04;	// Data width: 4 bits for SD CARD
	printf("MMC_CTL is: 0x%x\n", MMC_CTL);

	// Assuming function clock 150Mhz
	// function_clock / ( 4 * (CLK + 1))  < 400KHz   => CLK > 93.xx => CLK = 100
	MMC_CLK |= 0x264;	// DIV=4, CLKRT=200 (less than 400kHz clock)

	MMC_IM = (0x1 << 4);		// Only response timeout interrupt enabled for now 

	// Timeout configured only for responses
	//#warning: NO TIMEOUT CONFIGURED FOR DATA RESPONSE
	//#warning: RESPONSE TIMEOUT JUST 1 clock cycle
	MMC_CTOR = 0xFF;	// Max response timeout (255 cycles)	
	MMC_CTOD = 0x00;

	MMC_BLEN = 128;		// 128 byte blocks
	MMC_NBLK = 1;		// 1 block

	printf("MMC_CTL is: 0x%x\n", MMC_CTL);
	MMC_CTL &= 0xFFFFFFFC;	// Bring controller out of reset
	printf("MMC_CTL is: 0x%x\n", MMC_CTL);

	MMC_CLK |= 0x100;	// Enable clock to the card


	detect_card();



	return;
}

	

